import numpy as np


def fhan(x1, x2, h, r, p):
    y = x1 + h * x2
    d = h * h * r
    k = (1 + np.sqrt(1 + 8 * abs(y) / d)) / 2
    k = np.sign(k - np.floor(k)) + np.floor(k)
    if abs(y) <= d:
        u = -(y + h * x2) / d
    else:
        u = np.sign(y) * (1 - k / 2) - (x1 + k * h * x2) / (k - 1) / d
    u = r * val_limit(u, -1, 1)
    if abs(x2 + h * u) > p:
        u = (p * np.sign(x2) - x2) / h
        u = val_limit(u, -r, r)
    return u


def val_limit(x, a, b):
    y = x
    if y < a:
        y = a
    elif y > b:
        y = b
    return y
